
#include <string>
#include <sstream>
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <math.h>
#include <algorithm>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/angles.h>
#include <pcl/common/transforms.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/extract_indices.h>

using namespace std;
using namespace pcl;

void plyToPcd(string namefile)
{
    stringstream ss1;
    ss1<<namefile<<".ply";
    fstream fileSource(ss1.str().c_str(), ios::in);
    stringstream ss2;
    ss2<<namefile<<".pcd";
    fstream fileDestination(ss2.str().c_str(), ios::out);
    vector<string> lines;
    int j=0;
    while ( fileSource.good() )
    {
        string x;
        lines.push_back(x);
        getline (fileSource,lines.at(j++));
    }
    vector<string> linesFiltered;
    int start=0;
    for (int i=0;i<lines.size();i++)
    {
        if (lines.at(i).compare("end_header")==0)
            start=i+1;
    }
    for (int i=start;i<lines.size();i++)
    {
        if (lines.at(i).substr(0,2).compare("3 ")==0)
            break;
        linesFiltered.push_back(lines.at(i));
    }
    cout<<namefile<<" lines: "<<lines.size()<<endl;
    cout<<namefile<<" filtered lines: "<<linesFiltered.size()<<endl;
    fileDestination<<"VERSION .7\n";
    fileDestination<<"FIELDS x y z\n";
    fileDestination<<"SIZE 4 4 4\n";
    fileDestination<<"TYPE F F F\n";
    fileDestination<<"COUNT 1 1 1\n";
    fileDestination<<"WIDTH "<<linesFiltered.size()<<"\n";
    fileDestination<<"HEIGHT 1\n";
    fileDestination<<"VIEWPOINT 0 0 0 1 0 0 0\n";
    fileDestination<<"POINTS "<<linesFiltered.size()<<"\n";
    fileDestination<<"DATA ascii\n";
    for (int i=0;i<linesFiltered.size();i++)
    {
        fileDestination<<linesFiltered.at(i)<<endl;
    }
    //"end_header"I 21



    /*
                                  # .PCD v.7 - Point Cloud Data file format
                                  VERSION .7
                                  FIELDS x y z
                                  SIZE 4 4 4
                                  TYPE F F F
                                  COUNT 1 1 1
                                  WIDTH 8
                                  HEIGHT 1
                                  VIEWPOINT 0 0 0 1 0 0 0
                                  POINTS 8
                                  DATA ascii
                                  */
}

void voxel(PointCloud<PointXYZ>::Ptr cloud, float leafSize)
{
    //cout << "Voxelling "<< cloud->size() <<" points... "<<flush;
    VoxelGrid<PointXYZ> vg;
    vg.setInputCloud (cloud);
    vg.setLeafSize (leafSize,leafSize,leafSize);
    vg.filter (*cloud);
    //cout << cloud->points.size() << " Voxels\n";fileDestination<<";
}

void segmentMainPlane(PointCloud<PointXYZ>::Ptr cloud, double planeThreshold)
{
    //cout << "Main Plane Segmentation... " << flush;
    ModelCoefficients::Ptr coefficients (new ModelCoefficients);
    PointIndices::Ptr inliers (new PointIndices);
    SACSegmentation<PointXYZ> seg;
    seg.setOptimizeCoefficients (true);
    seg.setModelType (SACMODEL_PERPENDICULAR_PLANE);//(SACMODEL_PLANE);
    seg.setAxis (Eigen::Vector3f (0.0, 0.0, 1.0));//parallelo asse z
    seg.setEpsAngle(deg2rad(5.0f));
    seg.setMethodType (SAC_RANSAC);
    seg.setDistanceThreshold (planeThreshold);
    seg.setInputCloud (cloud);
    seg.segment (*inliers, *coefficients);
    ExtractIndices<PointXYZ> extract;
    extract.setInputCloud (cloud);
    extract.setIndices (inliers);
    extract.setNegative (true);
    extract.filter (*cloud);
    vector<int> indices;
    removeNaNFromPointCloud(*cloud, *cloud, indices);
    //cout << "OK! Segmented: now there are " << cloud->size  () << " points.\n";fileDestination<<";


}


void removeOutliers(PointCloud<PointXYZ>::Ptr source,PointCloud<PointXYZ>::Ptr target, int meanK, double stdDev)
{
    //cout << "Removing outliers... " << flush;
    StatisticalOutlierRemoval<PointXYZ> sor;
    sor.setInputCloud (source);
    sor.setMeanK (meanK);//2 Set the number of nearest neighbors to use for mean distance estimation.
    sor.setStddevMulThresh (stdDev);//1 Set the standard deviation multiplier for the distance threshold calculation.
    sor.filter (*target);
    //cout << "OK! " << cloud->size() << " points Loaded."<<endl;
}


void filterNormals(PointCloud<PointXYZ>::Ptr source,PointCloud<Normal>::Ptr target,double radius)
{
    NormalEstimationOMP<PointXYZ,Normal> ne;
    ne.setInputCloud(source);
    ne.setRadiusSearch(radius);
    ne.setSearchMethod(boost::shared_ptr<search::KdTree<PointXYZ> >(new search::KdTree<PointXYZ>));
    ne.compute(*target);
}

void removeClones(PointIndicesPtr source)
{
    cout<<"before:"<<source->indices.size()<<endl;
    PointIndicesPtr cleaned(new PointIndices);
    sort(source->indices.begin(),source->indices.end());
    cleaned->indices.push_back(source->indices.at(0));//inizializzazione
    for(int i=0;i<source->indices.size();i++)
        if(cleaned->indices.back()!=source->indices.at(i))
            cleaned->indices.push_back(source->indices.at(i));
    *source=*cleaned;
    cout<<"cleaned: "<<source->indices.size()<<endl;
}

void rayCastingFromTop(PointCloud<PointXYZ>::Ptr source,PointCloud<PointXYZ>::Ptr target,double leafSize, double radius)
{
    cout<<"source "<<source->size()<<endl;
    PointCloud<PointXYZ>::Ptr sourceDownsampled(new PointCloud<PointXYZ>);
    VoxelGrid<PointXYZ> vg;
    vg.setInputCloud (source);
    vg.setLeafSize (leafSize,leafSize,leafSize);
    vg.filter (*sourceDownsampled);
    cout<<"downsampled "<<sourceDownsampled->size()<<endl;
    PointIndicesPtr toDelete(new PointIndices);
    for(int i=0;i<sourceDownsampled->size();i++)
    {
        PointXYZ currentPoint=sourceDownsampled->at(i);
        for(int j=0;j<source->size();j++)
            if(source->at(j).z - currentPoint.z > radius&&
                    abs(source->at(j).x-currentPoint.x) <radius&&
                    abs(source->at(j).y-currentPoint.y) <radius
                    )
                toDelete->indices.push_back(j);
    }
    cout<<"indicesComputed"<<endl;
    ExtractIndices<PointXYZ> extract;
    extract.setInputCloud (source);
    removeClones(toDelete);
    extract.setIndices (toDelete);
    extract.setNegative (true);
    extract.filter (*target);
}

int main(int argc, char *argv[])
{
    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
    stringstream ss;
    string folder = argv[1];
    if (argv[5][0]=='s')     plyToPcd(argv[1]);
    //ss <<"../"<<folder<<"/points.pcd";
    ss <<folder<<".pcd";
    io::loadPCDFile(ss.str(),*cloud);
    //ss<<"/home/stefano/mod.ply";
    //PLYReader reader;
    //reader.read(ss.str(),*cloud);
    //io::loadPLYFile(ss.str(),*cloud);
    double leafSize=atof(argv[2]);
    if(leafSize>0) voxel(cloud,leafSize);
    double segmentPlaneThreshold=atof(argv[3]);
    if(segmentPlaneThreshold>0) segmentMainPlane(cloud,segmentPlaneThreshold);
    double removalMean=atof(argv[4]);
    if(removalMean>0) removeOutliers(cloud,cloud,removalMean,0.5*removalMean);
    //PointCloud<Normal>::Ptr normals(new PointCloud<Normal>);
    //filterNormals(cloud,normals,3);


    Eigen::Affine3f tr = getTransformation(0,0,0,0,rad2deg(45.0f),0);
    transformPointCloud(*cloud,*cloud,tr);

    if (argv[5][0]=='r')    rayCastingFromTop(cloud,cloud,5,5);

    //VIEWER
    visualization::PCLVisualizer viewer("PCL Viewer");
    viewer.setBackgroundColor (1, 1, 1);
    viewer.initCameraParameters ();
    visualization::PointCloudColorHandlerCustom<PointXYZ> rgb(cloud,0,0,255);
    viewer.addPointCloud<PointXYZ> (cloud, rgb, "cloud");
    //viewer.addPointCloudNormals<PointXYZ,Normal>(cloud,normals,10,10,"normals");
    viewer.resetCamera();
    while (!viewer.wasStopped ())	{ viewer.spin ();}
    return(0);


}//END MAIN










